/*
 * PhysicSpaceBridge.cpp
 *
 *  Created on: Sep 28, 2009
 *      Author: Falken
 */

#include <jni.h>
#include <iostream.h>
#include <math.h>
#include "PhysicsSpaceBridge.h"
#include "BasicMotionState.h"
#include "btBulletDynamicsCommon.h"
#include "btBulletCollisionCommon.h"
#include "BulletCollision/Gimpact/btGImpactShape.h"
#include "BulletMultiThreaded/btGpu3DGridBroadphase.h"

PhysicsSpaceBridge::PhysicsSpaceBridge(JNIEnv* env, jobject physicsSpace) {
	this->javaPhysicsSpace=env->NewGlobalRef(physicsSpace);
	rigidBodyIndex = 0;
	constraintIndex = 0;

	if(!initJavaMethodHandles(env))
	{
		free(this);
		return;
	}

	///collision configuration contains default setup for memory, collision setup
	btCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
	//m_collisionConfiguration->setConvexConvexMultipointIterations();

	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	btCollisionDispatcher* dispatcher = new	btCollisionDispatcher(collisionConfiguration);

	btVector3 min = btVector3(-1500,-1500,-1500);
	btVector3 max = btVector3(1500,1500,1500);

	btBroadphaseInterface* broadphase = new btDbvtBroadphase();
//	btBroadphaseInterface* broadphase = new btGpu3DGridBroadphase(
//			min,max,
//			20,20,20,
//			10000, 1000, 25);

//	btBroadphaseInterface* broadphase = new btAxisSweep3(min,max);
	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

	dynWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);

	dynWorld->setGravity(btVector3(0,-10,0));

	bulletRigidBodies[-1]=new btRigidBody(0,new btDefaultMotionState(),new btCompoundShape(),btVector3(0,0,0));
}

bool PhysicsSpaceBridge::initJavaMethodHandles(JNIEnv* env)
{
	physicsSpaceClass = env->FindClass("com/jmex/physics/impl/bullet/BulletPhysicsSpace");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_physicsSpace_applyPhysicsMovement = env->GetMethodID(physicsSpaceClass,"applyPhysicsMovement","([Lcom/jmex/physics/impl/bullet/BulletRigidBody;[D[D[D[D[D[D[D)V");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	rigidBodyClass = env->FindClass("com/jmex/physics/impl/bullet/BulletRigidBody");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_rigidBody_constructor = env->GetMethodID(rigidBodyClass,"<init>","()V");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}
	method_rigidBody_getNativeBulletHandle = env->GetMethodID(rigidBodyClass,"getNativeBulletHandle","()J");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_rigidBody_getShapes = env->GetMethodID(rigidBodyClass,"getShapes","()[Lcom/jmex/physics/impl/bullet/geometry/BulletGeometry;");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	geometryClass = env->FindClass("com/jmex/physics/impl/bullet/geometry/BulletGeometry");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_geometry_getLocalTranslation = env->GetMethodID(geometryClass,"getLocalTranslationAsArray","()[F");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_geometry_getLocalRotation = env->GetMethodID(geometryClass,"getLocalRotationAsArray","()[F");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_geometry_getLocalScale = env->GetMethodID(geometryClass,"getLocalScaleAsArray","()[F");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_geometry_getShape = env->GetMethodID(geometryClass,"getShapeOrdinal","()I");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	meshClass = env->FindClass("com/jmex/physics/impl/bullet/geometry/BulletMesh");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_mesh_getVerticesArray = env->GetMethodID(meshClass,"getVerticesArray","()[F");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_mesh_getTrianglesArray = env->GetMethodID(meshClass,"getTrianglesArray","()[I");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	jointClass = env->FindClass("com/jmex/physics/impl/bullet/joints/BulletJoint");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	method_joint_getNativeBulletHandle = env->GetMethodID(jointClass,"getNativeBulletHandle","()J");
	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return false;
	}

	IAE = env->FindClass("java/lang/IllegalArgumentException");

	return true;
}

jlong PhysicsSpaceBridge::buildRigidBody(JNIEnv* env,jobject javaRB, btVector3 pos, btQuaternion rot, btVector3 worldScale, btTransform com, btVector3 linVel, btVector3 angVel, float mass)
{
	btRigidBody* rb;
	cout << "Entered rigidBody" << endl;
	jlong rbHandle=env->CallLongMethod(javaRB,method_rigidBody_getNativeBulletHandle);
	if(rbHandle!=-1)
		rb = bulletRigidBodies[rbHandle];

	cout << "RBHandle = " << rbHandle << endl;

	if(rbHandle!=-1)
	{
		cout << "removed rigid body" << endl;
		dynWorld->removeRigidBody(rb);
	}

	btCollisionShape* shape;
	bool isCompound = false;

	jobjectArray shapeArray = (jobjectArray)env->CallObjectMethod(javaRB,method_rigidBody_getShapes);
	cout << "Got shape array" << endl;
	int length = env->GetArrayLength(shapeArray);
	cout << "Shape array length=" << length << endl;

	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return -1;
	}
	if(length==0)
	{
		cout << "length is 0 . . . throwing IAE" << endl;
		//not sure what to do here.
		jclass IAE = env->FindClass("java/lang/IllegalArgumentException");
		env->ThrowNew(IAE,"Cannot build a rigid body with no collision shapes.  (at least . . . not right now!)");
		return -1;
	} else {
		if(length>1)
		{
			isCompound = true;
			shape = new btCompoundShape();
		}

		if(isCompound)
			cout << "Built a compound shape." << endl;

		btCollisionShape* tempShape;

		for(int loop=0; loop<length; loop++)
		{
			cout << "Looping through shapes: loop=" << loop << endl;
			jobject geom = env->GetObjectArrayElement(shapeArray,loop);
			cout << "Got geometry" << endl;
			jfloat buf[4];

			env->GetFloatArrayRegion((jfloatArray)env->CallObjectMethod(geom,method_geometry_getLocalRotation),0,4,buf);
			cout << "Got rotation array" << buf[0] << "," << buf[1] << "," << buf[2] << endl;
			btQuaternion rotation(buf[0],buf[1],buf[2],buf[3]);
			cout << "Build rotation" << endl;

			env->GetFloatArrayRegion((jfloatArray)env->CallObjectMethod(geom,method_geometry_getLocalTranslation),0,3,buf);
			cout << "Got translation array" << buf[0] << "," << buf[1] << "," << buf[2] << endl;
			btVector3 translation(buf[0],buf[1],buf[2]);
			cout << "Built translation" << endl;

			if((rotation.getAngle()!=0 || translation.length()!=0) && !isCompound)
			{
				cout << "Shape is not on center" << endl;
				isCompound = true;
				shape = new btCompoundShape();
			}
			env->GetFloatArrayRegion((jfloatArray)env->CallObjectMethod(geom,method_geometry_getLocalScale),0,3,buf);
			cout << "Got scale array " << buf[0] << "," << buf[1] << "," << buf[2] << endl;
			btVector3 scale = btVector3(buf[0],buf[1],buf[2]);
			cout << "Build scale" << endl;

			switch(env->CallIntMethod(geom,method_geometry_getShape))
			{
				case 0://box
					cout << "creating box" << endl;
					scale*=worldScale;
					tempShape=new btBoxShape(btVector3(0.5,0.5,0.5)*=scale);
				break;
				case 1://sphere
					cout << "creating sphere" << endl;
					tempShape=new btSphereShape(1);
					tempShape->setLocalScaling(scale*=worldScale);
				break;
				case 2://cylinder
					cout << "creating cylinder" << endl;
					scale*=worldScale;
					tempShape=new btCylinderShapeZ(btVector3(1,1,0.5)*=scale);
				break;
				case 3://capsule
					cout << "creating capsule" << endl;
					tempShape=new btCapsuleShapeZ(1,1);
					tempShape->setLocalScaling(scale*=worldScale);
				break;
				case 4://mesh
					cout << "creating mesh" << endl;
					jarray tempArray = (jarray)env->CallObjectMethod(geom,method_mesh_getVerticesArray);
					int numVertices = env->GetArrayLength(tempArray)/3;
					float* verticesTemp = new float[numVertices*3];
					env->GetFloatArrayRegion((jfloatArray)tempArray,0,numVertices*3,verticesTemp);
					btScalar* vertices = new btScalar[numVertices*3];
					for(int loop=0; loop<numVertices*3; loop++)
					{
						vertices[loop]=(btScalar)verticesTemp[loop];
					}

					tempArray = (jarray)env->CallObjectMethod(geom,method_mesh_getTrianglesArray);
					int numTriangles = (env->GetArrayLength(tempArray))/3;
					jint* trianglesTemp = new jint[numTriangles*3];
					env->GetIntArrayRegion((jintArray)tempArray,0,numTriangles*3,trianglesTemp);
					int* triangles = new int[numTriangles*3];
					for(int loop=0; loop<numTriangles*3; loop++)
					{
						triangles[loop]=(int)trianglesTemp[loop];
					}
					btTriangleIndexVertexArray* meshData = new btTriangleIndexVertexArray(numTriangles,triangles,sizeof(int)*3,numVertices,vertices,sizeof(btScalar)*3);
					tempShape = new btGImpactMeshShape(meshData);
					tempShape->setLocalScaling(scale*=worldScale);
					((btGImpactMeshShape*)tempShape)->updateBound();
//					tempShape = new btScaledBvhTriangleMeshShape(actualMesh, scale*=worldScale);
				case 5://ray

				break;
				default:
					env->ThrowNew(IAE,"Cannot build a rigid body with no collision shapes.  (at least . . . not right now!)");
					return -1;
				break;
			}
			if(!isCompound)
			{
				cout << "No compound shape.  Just the base." << endl;
				shape=tempShape;
			} else {
				cout << "Compound shape.  Adding as child" << endl;
				btTransform xform(rotation,translation-=com.getOrigin());
				cout << "Created translation " << translation.x() << "," << translation.y() << "," << translation.z() << endl;
				((btCompoundShape*)shape)->addChildShape(xform,tempShape);
				cout << "Successfully added child" << endl;
			}
		}
	}

	if(env->ExceptionCheck())
	{
		env->Throw(env->ExceptionOccurred());
		return -1;
	}

	cout << "Creating new rigidbody." << endl;

	if(rbHandle==-1)
		rbHandle=rigidBodyIndex++;

	javaRigidBodies[rbHandle]=env->NewGlobalRef(javaRB);

	btVector3 localInertia = btVector3();
	shape->calculateLocalInertia(mass,localInertia);

	cout << "Local inertia is " << localInertia.x() << "," << localInertia.y() << "," << localInertia.z() << endl;

	BasicMotionState* ms = new BasicMotionState(btTransform(rot,pos) * com);

	rb = new btRigidBody(mass,ms,shape,localInertia);

	bulletRigidBodies[rbHandle]=rb;

	cout << "Setting velocities" << endl;


	rb->setLinearVelocity(linVel);
	rb->setAngularVelocity(angVel);

	cout << "Adding to dynamicsworld" << endl;

	dynWorld->addRigidBody(rb);

	cout << "Added.  We're good!" << endl << "********************" << endl;

	return rbHandle;
}

jlong PhysicsSpaceBridge::buildJoint(JNIEnv* env, jobject javaJoint, int jointType,
		btTransform refA, btTransform refB, jlong rbAHandle, jlong rbBHandle, jboolean collideWithSelf)
{
	btTypedConstraint* con = 0;
	cout << "Entered buildJoint" << endl;
	jlong conHandle=env->CallLongMethod(javaJoint,method_joint_getNativeBulletHandle);
	if(conHandle!=-1)
		con = bulletConstraints[conHandle];

	cout << "ConstraintHandle = " << conHandle << endl;

	if(conHandle!=-1)
	{
		dynWorld->removeConstraint(con);
		bulletConstraints.erase(conHandle);
	}

	btRigidBody* rbA = bulletRigidBodies[rbAHandle];
	btRigidBody* rbB = bulletRigidBodies[rbBHandle];
	btVector3 axisA = btVector3(1,0,0);
	btVector3 axisB = btVector3(1,0,0);

	switch(jointType)
	{
		case 0: //Fixed
			con = new btGeneric6DofConstraint(*rbA,*rbB,refA,refB,false);
			((btGeneric6DofConstraint*)con)->setLinearLowerLimit(btVector3(0,0,0));
			((btGeneric6DofConstraint*)con)->setLinearUpperLimit(btVector3(0,0,0));
		break;
		case 1: //Slider
			con = new btSliderConstraint(*rbA,*rbB,refA,refB,false);
		break;
		case 2: //Hinge
			con = new btHingeConstraint(*rbA,*rbB,refA,refB);
		break;
		case 3: //Hinge2
			axisA=btTransform(refA.getRotation())*axisA;
			axisB=refB*axisB;
			con = new btHinge2Constraint(*rbA,*rbB,refA.getOrigin(),axisA,axisB);
		break;
		case 4: //ConeTwist
			con = new btConeTwistConstraint(*rbA,*rbB,refA,refB);
		break;
		case 5: //6dof
			con = new btGeneric6DofSpringConstraint(*rbA,*rbB,refA,refB,false);
		break;
	}

	if(conHandle==-1)
		conHandle=constraintIndex++;

	jointTypes[conHandle]=jointType;
	javaJoints[conHandle]=javaJoint;

	if(con!=0)
	{
		bulletConstraints[conHandle]=con;
		if(collideWithSelf)
			dynWorld->addConstraint(con,false);
		else
			dynWorld->addConstraint(con,true);
	}

	return conHandle;
}

float PhysicsSpaceBridge::getPosition(jlong jointHandle, int axisIndex)
{
	btTypedConstraint* con = bulletConstraints[jointHandle];
	switch(jointTypes[jointHandle])
	{
		case 1://Slider
			return (con->getRigidBodyA().getWorldTransform().getOrigin()
					- con->getRigidBodyB().getWorldTransform().getOrigin())
					.length();
		break;
		case 2://Hinge
			return ((btHingeConstraint*)con)->getHingeAngle();
		break;
		case 3://Hinge2
			return ((btHinge2Constraint*)con)->getAngle(axisIndex);
		break;

		case 5://6Dof
			//Since we're only doing rotational for this one, this will always be an angle
			return ((btGeneric6DofConstraint*)con)->getAngle(axisIndex);
		break;
		default:
			return 0;
	}
}

float PhysicsSpaceBridge::getVelocity(jlong jointHandle, int axisIndex)
{
	btTypedConstraint* con = bulletConstraints[jointHandle];
	switch(jointTypes[jointHandle])
	{
		case 1:
			btVector3 relVel = con->getRigidBodyA().getLinearVelocity() - con->getRigidBodyB().getLinearVelocity();
			btVector3 localDir = con->getRigidBodyA().getWorldTransform().getOrigin() - con->getRigidBodyB().getWorldTransform().getOrigin();
			btQuaternion rot = btQuaternion(localDir.normalize().cross(btVector3(1,0,0)),localDir.normalize().angle(btVector3(1,0,0)));
			relVel = btTransform(rot) * relVel;
			return relVel.x();
		break;
		return 0;
	}
}

void PhysicsSpaceBridge::updateJointAxis(jlong jointHandle, int axisIndex,
			float availableAccel, float desiredVel, float posMin, float posMax)
{
	cout << "updateJointAxis: joint=" << jointHandle << " axisIndex=" << axisIndex << endl;
	btTypedConstraint* con = bulletConstraints[jointHandle];
	btScalar totalMass;
	float angMomentum;
	btVector3 ang;
	switch(jointTypes[jointHandle])
	{
		case 1: //Slider
			((btSliderConstraint*)con)->setLowerLinLimit(posMin);
			((btSliderConstraint*)con)->setUpperLinLimit(posMax);
			((btSliderConstraint*)con)->setTargetLinMotorVelocity(desiredVel);
			totalMass=0;
			if(con->getRigidBodyA().getInvMass()!=0)
				totalMass+=1/con->getRigidBodyA().getInvMass();
			if(con->getRigidBodyB().getInvMass()!=0)
				totalMass+=1/con->getRigidBodyB().getInvMass();
			((btSliderConstraint*)con)->setMaxLinMotorForce(availableAccel*totalMass);
		break;
		case 2: //Hinge
			((btHingeConstraint*)con)->setLimit(posMin,posMax);
			if(availableAccel==0)
			{
				((btHingeConstraint*)con)->enableAngularMotor(false,0,0);
				break;
			}
			angMomentum = 0;
			if(con->getRigidBodyA().getInvMass()!=0)
			{
				float distFromPivot = (con->getRigidBodyA().getWorldTransform().getOrigin()
							- ((btHingeConstraint*)con)->getAFrame().getOrigin()).length();
				angMomentum+=(1/con->getRigidBodyA().getInvMass()) * distFromPivot;
			}
			if(con->getRigidBodyB().getInvMass()!=0)
			{
				float distFromPivot = (con->getRigidBodyB().getWorldTransform().getOrigin()
							- ((btHingeConstraint*)con)->getBFrame().getOrigin()).length();
				angMomentum+=(1/con->getRigidBodyB().getInvMass()) * distFromPivot;
			}
    		ang = btVector3(0,0,1);
    		ang = btTransform(((btHingeConstraint*)con)->getAFrame().getBasis()) * ang;
    		ang *= availableAccel;
    		ang = btTransform(con->getRigidBodyA().getInvInertiaTensorWorld()) * ang;
    		angMomentum+=ang.length();

    		ang = btVector3(0,0,1);
    		ang = btTransform(((btHingeConstraint*)con)->getBFrame().getBasis()) * ang;
    		ang *= availableAccel;
    		ang = btTransform(con->getRigidBodyB().getInvInertiaTensorWorld()) * ang;
    		angMomentum+=ang.length();

			((btHingeConstraint*)con)->enableAngularMotor(true, desiredVel, availableAccel/angMomentum);
			con->getRigidBodyA().activate(true);
			con->getRigidBodyB().activate(true);
		break;
		case 3: //Hinge2
			((btHinge2Constraint*)con)->setLimit(axisIndex,posMin,posMax);
			//need to get force working.
		break;
		case 4: //ConeTwist
			//gotta figure out how this works ANYWAY
		break;
		case 5: //6dof
			((btGeneric6DofConstraint*)con)->setLimit(axisIndex,posMin,posMax);
		break;
	}
}

void PhysicsSpaceBridge::setRigidBodyInfo(jlong rbHandle,
		float tx, float ty, float tz,
		float rx, float ry, float rz, float rw,
		float lvx, float lvy, float lvz,
		float avx, float avy, float avz)
{
	cout << "entered setRigidBodyInfo" << endl;
	bulletRigidBodies[rbHandle]->setWorldTransform(btTransform(btQuaternion(rx,ry,rz,rw),btVector3(tx,ty,tz)));
	bulletRigidBodies[rbHandle]->setLinearVelocity(btVector3(lvx,lvy,lvz));
	bulletRigidBodies[rbHandle]->setAngularVelocity(btVector3(avx,avy,avz));
	bulletRigidBodies[rbHandle]->getMotionState()->setWorldTransform(btTransform(btQuaternion(rx,ry,rz,rw),btVector3(tx,ty,tz)));
	if(!bulletRigidBodies[rbHandle]->isStaticOrKinematicObject())
		bulletRigidBodies[rbHandle]->activate(true);
	cout << "done setting rigid body info" << endl;
}

void PhysicsSpaceBridge::applyForces(jlong rbHandle, float fx, float fy, float fz, float px, float py, float pz, float tx, float ty, float tz)
{
	if(isnan(fx))
	{
		cout << "clearing forces" << endl;
		btVector3 torque = bulletRigidBodies[rbHandle]->getTotalTorque();
		bulletRigidBodies[rbHandle]->clearForces();
		bulletRigidBodies[rbHandle]->applyTorque(torque);
	} else {
		cout << "applying force " << fx << "," << fy << "," << fz << " at " << px << "," << py << "," << pz << endl;
		bulletRigidBodies[rbHandle]->applyImpulse(btVector3(fx,fy,fz),btVector3(px,py,pz));
	}
	if(isnan(tx))
	{
		cout << "clearing torque" << endl;
		btVector3 force = bulletRigidBodies[rbHandle]->getTotalForce();
		bulletRigidBodies[rbHandle]->clearForces();
		bulletRigidBodies[rbHandle]->applyCentralForce(force);
	} else {
		cout << "applying torque " << tx << "," << ty << "," << tz << endl;
		bulletRigidBodies[rbHandle]->applyTorqueImpulse(btVector3(tx,ty,tz));
	}
}

void PhysicsSpaceBridge::step(JNIEnv* env,float timestep)
{
	dynWorld->stepSimulation(timestep,2);

	int dirtyCount=0;

	map<jlong,btRigidBody*>::iterator i = bulletRigidBodies.begin();

	for(;i!=bulletRigidBodies.end();i++)
	{
		if(i->second->isStaticOrKinematicObject() || !((BasicMotionState*)i->second->getMotionState())->dirty)
			continue;
		dirtyCount++;
	}

	rigidBodyClass = env->FindClass("com/jmex/physics/impl/bullet/BulletRigidBody");

	jobject rbObj = env->NewObject(rigidBodyClass,method_rigidBody_constructor);

	jobjectArray rb = env->NewObjectArray(dirtyCount,rigidBodyClass,rbObj);

	double* tx = new double[dirtyCount];
	double* ty = new double[dirtyCount];
	double* tz = new double[dirtyCount];
	double* rx = new double[dirtyCount];
	double* ry = new double[dirtyCount];
	double* rz = new double[dirtyCount];
	double* rw = new double[dirtyCount];

	i = bulletRigidBodies.begin();
	dirtyCount=0;
	btTransform xform;
	btQuaternion rot;
	btVector3 origin;
	for(;i!=bulletRigidBodies.end();i++)
	{
		if(i->second->isStaticOrKinematicObject() || !((BasicMotionState*)i->second->getMotionState())->dirty)
			continue;
		i->second->getMotionState()->getWorldTransform(xform);
		env->SetObjectArrayElement(rb,dirtyCount,javaRigidBodies[i->first]);
		origin=xform.getOrigin();
		tx[dirtyCount]=origin.x();
		ty[dirtyCount]=origin.y();
		tz[dirtyCount]=origin.z();
		rot=xform.getRotation();
		rx[dirtyCount]=rot.x();
		ry[dirtyCount]=rot.y();
		rz[dirtyCount]=rot.z();
		rw[dirtyCount]=rot.w();
		dirtyCount++;
	}

	jdoubleArray txArr = env->NewDoubleArray(dirtyCount);
	jdoubleArray tyArr = env->NewDoubleArray(dirtyCount);
	jdoubleArray tzArr = env->NewDoubleArray(dirtyCount);
	jdoubleArray rxArr = env->NewDoubleArray(dirtyCount);
	jdoubleArray ryArr = env->NewDoubleArray(dirtyCount);
	jdoubleArray rzArr = env->NewDoubleArray(dirtyCount);
	jdoubleArray rwArr = env->NewDoubleArray(dirtyCount);

	env->SetDoubleArrayRegion(txArr,0,dirtyCount,tx);
	env->SetDoubleArrayRegion(tyArr,0,dirtyCount,ty);
	env->SetDoubleArrayRegion(tzArr,0,dirtyCount,tz);
	env->SetDoubleArrayRegion(rxArr,0,dirtyCount,rx);
	env->SetDoubleArrayRegion(ryArr,0,dirtyCount,ry);
	env->SetDoubleArrayRegion(rzArr,0,dirtyCount,rz);
	env->SetDoubleArrayRegion(rwArr,0,dirtyCount,rw);

	setWorldTransform(env,rb,txArr,tyArr,tzArr,rxArr,ryArr,rzArr,rwArr);
}

void PhysicsSpaceBridge::setWorldTransform(JNIEnv* env,
										   jobjectArray rb,
										   jdoubleArray tx,
										   jdoubleArray ty,
										   jdoubleArray tz,
										   jdoubleArray rx,
										   jdoubleArray ry,
										   jdoubleArray rz,
										   jdoubleArray rw)
{
	env->CallVoidMethod(javaPhysicsSpace,method_physicsSpace_applyPhysicsMovement,
			rb,tx,ty,tz,rx,ry,rz,rw);
}

PhysicsSpaceBridge::~PhysicsSpaceBridge() {
	for(map<jlong,jobject>::iterator iter=javaRigidBodies.begin();iter!=javaRigidBodies.end();iter++)
	{
		free(iter->second);
	}
	for(map<jlong,btRigidBody*>::iterator iter=bulletRigidBodies.begin();iter!=bulletRigidBodies.end();iter++)
		free(iter->second);
}
